/**
 * @file:          ThreeLoopPidFocV1_0.h
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.06.14
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.06.14,00:35:44
 */

#ifndef _THREELOOPPIDFOCV1_0_H_
#define _THREELOOPPIDFOCV1_0_H_

/* Include Files **************************************************************/
#include "Datatype.h"
#include "Pid.h"
#include "Foc.h"
#include "DriverList.h"

/* Macro Definition ***********************************************************/
#define FILTER_NOTCH_NUM 3

/* Data Type Define ***********************************************************/
enum ControlMode
{
    CONTROL_MODE_OPEN_LOOP_ELEC_ANGLE_FREE_CONTROL = 0,
    CONTROL_MODE_OPEN_LOOP_CONTROL,
    CONTROL_MODE_CURRENT_CONTROL,
    CONTROL_MODE_SPEED_CONTROL,
    CONTROL_MODE_POSITION_CONTROL,
};
enum ControlAlgorithm
{
    ControlAlgorithm_SeriesPidLeso = 1,
    ControlAlgorithm_ParallelPidLeso = 2,
    ControlAlgorithm_FhanLeso = 3,
};
enum VoltageFeedforwardMode
{
    VoltageFeedforwardMode_SpdRef = 0,
    VoltageFeedforwardMode_SpdFdb = 1,
};
enum PosAcquireMethod
{
    PosAcquireMethod_DirectFromEncoder = 0,
    PosAcquireMethod_LESO_z1 = 1,
};
enum SpdAcquireMethod
{
    SpdAcquireMethod_DirectFromEncoder = 0,
    SpdAcquireMethod_MovingAverage = 1,
    SpdAcquireMethod_FIR_FirstOrder = 2,
    SpdAcquireMethod_LESO_z2 = 3,
};
typedef struct ThreeLoopPidFocV1_0
{
    /* public data */
    StepperMotor *motor;
    Pid pos_loop_pid;
    Pid spd_loop_pid;
    Pid id_loop_pid;
    Pid iq_loop_pid;
    Fhan fhan_controller;
    LESO leso;
    Foc foc;
    FieldWeakeningControl fwc;
    // Filter_MovingAverage spd_fdb_filter_moving_average;
    Filter_FirstOrder speed_filter;
    Filter_FirstOrder ialpha_filter;
    Filter_FirstOrder ibeta_filter;
    Filter_Notch spd_out_filter_notch[FILTER_NOTCH_NUM];
    Filter_BinarySecondOrder spd_out_filter_binary_second_order;
    Filter_BinarySecondOrder z3_filter_binary_second_order;
    TrackingDifferentiator td;

    // MotorRippleCompensator motor_ripple_compensator;
    float param_pos_period;
    float param_cur_period;

    enum ControlMode param_control_mode;
    enum ControlAlgorithm param_control_algorithm;
    enum PosAcquireMethod param_pos_acquire_method;
    enum SpdAcquireMethod param_spd_acquire_method;
    uint16 param_single_turn_mode;

    float32 param_pos_control_period_s;
    float32 param_cur_control_period_s;
    uint16 param_en_all_control_switch;
    uint16 param_en_feedforward_speed;
    uint16 param_en_feedforward_idiq;
    uint16 param_en_feedforward_uduq;
    uint16 param_speed_feedforward_mode;
    float32 param_feedforward_speed_limit;
    float32 param_feedforward_idiq_limit;
    float32 param_feedforward_uduq_limit;

    float32 param_fhan_controller_c;

    float32 param_motor_R;
    float32 param_motor_L;

    float32 in_pos_cmd;
    float32 in_spd_cmd;
    float32 in_acc_cmd;
    float32 in_id_cmd;
    float32 in_iq_cmd;
    float32 in_ud_cmd;
    float32 in_uq_cmd;
    int32_t in_encoder;
    float32 in_pos_fdb;
    float32 in_spd_fdb;
    float32 in_elec_angle_fdb;
    float32 in_pos_fdb_abs;
    float32 in_ialpha_fdb;
    float32 in_ibeta_fdb;

    float32 in_custom_pos_cmd;
    float32 in_custom_spd_cmd;
    float32 in_custom_id_cmd;
    float32 in_custom_iq_cmd;
    float32 in_custom_ud_cmd;
    float32 in_custom_uq_cmd;
    float32 in_custom_elecangle_cmd;

    float32 *speed_source;
    float32 *speed_source_ref;
    float32 *speed_source_fdb;

    float32 out_u_A;
    float32 out_u_B;

    float32 spd_feedforward;
    float32 id_feedforward;
    float32 iq_feedforward;
    float32 ud_feedforward;
    float32 uq_feedforward;
    float32 pos_fdb_ctrl;
    float32 spd_fdb_ctrl;
    float32 *pos_fdb_ctrl_link;
    float32 *spd_fdb_ctrl_link;
    float32 spd_fdb_ctrl_last;
    float32 iq_ref_total_out;
    float32 z3_filt;
    float32 spd_loop_pid_out_tmp;
    float32 spd_loop_pid_in_ref_tmp;
    float32 spd_fdb_ctrl_tmp;
    float32 iqfdf_before_u;
    float32 iqfdf_leso_z3_tmp;
    int32 steps_cmd;
    float Is;
    float theta_delta;

    /* private data */

    /* public method */
    void (*ConfigParam)(struct ThreeLoopPidFocV1_0 *self);
    void (*UpdateParam)(struct ThreeLoopPidFocV1_0 *self);
    uint8 (*Init)(struct ThreeLoopPidFocV1_0 *self);
    void (*Run)(struct ThreeLoopPidFocV1_0 *self);
    void (*Clear)(struct ThreeLoopPidFocV1_0 *self);

    /* private method */

} ThreeLoopPidFocV1_0;

/* Variable Declare ***********************************************************/

/* Function Declare ***********************************************************/
void ThreeLoopPidFocV1_0_UpdateParam(struct ThreeLoopPidFocV1_0 *self);
uint8 ThreeLoopPidFocV1_0_Init(struct ThreeLoopPidFocV1_0 *self);
void ThreeLoopPidFocV1_0_SeriesPidLeso_Run(struct ThreeLoopPidFocV1_0 *self);
void ThreeLoopPidFocV1_0_SeriesPidLeso_Clear(struct ThreeLoopPidFocV1_0 *self);
void ThreeLoopPidFocV1_0_ParallelPidLeso_Run(struct ThreeLoopPidFocV1_0 *self);
void ThreeLoopPidFocV1_0_ParallelPidLeso_Clear(struct ThreeLoopPidFocV1_0 *self);
void ThreeLoopPidFocV1_0_FhanLeso_Run(struct ThreeLoopPidFocV1_0 *self);
void ThreeLoopPidFocV1_0_FhanLeso_Clear(struct ThreeLoopPidFocV1_0 *self);

#endif